/*
 * @Author: your name
 * @Date: 2023-11-11 11:08:15
 * @LastEditTime: 2023-11-30 16:54:32
 * @LastEditors: Please set LastEditors
 * @Description: In User Settings Edit
 * @FilePath: \Angle\User\UserThreads\user_tasks.c
 */
#include "ahrs.h"
#include "adaption_imu.h"
#include "adaption_magnetometer.h"
#include "system_support.h"
#include "system.h"
#include "pose.h"
#define SEND_TASK_TAG "SEND_TASK"

extern IMU_Typedef IMU;
extern MagnetometerTypedef Magnetometer;

sensor_imu_msg user_imu_msg;
sensor_mag_msg user_mag_msg;
ahrs_angle_msg user_angle_msg;// 用户层面调用
ahrs_angle_msg task_angle_msg;// 姿态解算层面调用

Pose_Module pose;
uint8_t    TxDat[30];
uint64_t AttitudeCalTime[3] = {0};

void AttitudeCalFunc(void *argument){
    // 这部分可能需要卸载main开头
    //Ahrs_Init_Para(&ahrs[0]);
    //ESKF_Init_MCH(&ahrs[0]);
    vTaskDelay(200);
    initPose_Module(&pose);
    pose.interface.data.a_x = &user_imu_msg.acc[0];
    pose.interface.data.a_y = &user_imu_msg.acc[1];
    pose.interface.data.a_z = &user_imu_msg.acc[2];
    pose.interface.data.g_x = &user_imu_msg.gyro[0];
    pose.interface.data.g_y = &user_imu_msg.gyro[1];
    pose.interface.data.g_z = &user_imu_msg.gyro[2];
    pose.interface.data.m_x = &user_mag_msg.mag[0];
    pose.interface.data.m_y = &user_mag_msg.mag[1];
    pose.interface.data.m_z = &user_mag_msg.mag[2];
    for(;;){
        osSemaphoreWait(System.UserSems.AttitudeCalSem,0xffffffff);
        xQueuePeek(System.UserQueues.ImuDataQueue, &user_imu_msg, 0);
        xQueuePeek(System.UserQueues.MagDataQueue, &user_mag_msg, 0);
        //memcpy(ahrs->AccRel,user_imu_msg.acc,3);
        //memcpy(ahrs->GyrRel,user_imu_msg.gyro,3);
        //memcpy(ahrs->MagRel,user_mag_msg.mag,3);
        //ESKF_Attitude_Calc_MCH(&ahrs[0]);
        AttitudeCalTime[2] = Get_SystemTimer_us();
		UnitConversions(&pose);
        calculatePose_Module(&pose,0.002);// 耗时大约162us。
		task_angle_msg.angle_degree[0]= pose.data.rol;
		task_angle_msg.angle_degree[1]= pose.data.pit;
		task_angle_msg.angle_degree[2]= pose.data.yaw;
        
        xQueueOverwrite(System.UserQueues.AngleDataQueue, &task_angle_msg.angle_degree);
        AttitudeCalTime[0] = AttitudeCalTime[2] - AttitudeCalTime[1];
        AttitudeCalTime[1] = AttitudeCalTime[2];
		
    }
}


void SendDataTaskFunc(void *argument){
    //TODO:信号量控制周期
    int cnt = 0;
    for(; ;){
		osSemaphoreWait(System.UserSems.SendDataSem,0xffffffff);
        if(cnt % 2 == 0 && SendStatusData() != HAL_OK){
            elog_warn(SEND_TASK_TAG, "send data error");
			LED_Toggle(LED_Color_Green);
        }
		else if(cnt % 5 == 0 && SendSensor1Data() != HAL_OK){
			elog_warn(SEND_TASK_TAG, "send data error");
			LED_Toggle(LED_Color_Green);
        }
		cnt ++;
    }
}

uint8_t SendStatusData(void)
{
	
	xQueuePeek(System.UserQueues.AngleDataQueue, &user_angle_msg, 0);
	vs16 _temp;
	vs32 _temp2;
    TxDat[0] = 0xAA;
	TxDat[1] = 0xAA;
	TxDat[2] = 0x01;
	TxDat[3] = 12;
	_temp = (s16)(user_angle_msg.angle_degree[0]*100.0f);
	TxDat[4] = BYTE1(_temp);
	TxDat[5] = BYTE0(_temp);

	_temp = (s16)(user_angle_msg.angle_degree[1]*100.0f);
	TxDat[6] = BYTE1(_temp);
	TxDat[7] = BYTE0(_temp);

	_temp = (s16)(user_angle_msg.angle_degree[2]*100.0f);
	TxDat[8] = BYTE1(_temp);
	TxDat[9] = BYTE0(_temp);

	_temp2 = (s32)0;
	TxDat[10] = BYTE3(_temp2);
	TxDat[11] = BYTE2(_temp2);
	TxDat[12] = BYTE1(_temp2);
	TxDat[13] = BYTE0(_temp2);
	TxDat[14] = 0;		//模式
	TxDat[15] = 0;		//

	uint8_t sum = 0;
	for(uint8_t i=0; i<16; i++) sum += TxDat[i];
	TxDat[16] = sum;
	return HAL_UART_Transmit_DMA(&huart1, TxDat, 17);
}


uint8_t SendSensor1Data(void)
{
    xQueuePeek(System.UserQueues.ImuDataQueue, &user_imu_msg, 0);
    xQueuePeek(System.UserQueues.MagDataQueue, &user_mag_msg, 0);
	vs16 _temp;
    TxDat[0] = 0xAA;
	TxDat[1] = 0xAA;
	TxDat[2] = 0x02;
	TxDat[3] = 18;
	_temp = user_imu_msg.acc[0]*100;
	TxDat[4] = BYTE1(_temp);
	TxDat[5] = BYTE0(_temp);
	_temp = user_imu_msg.acc[1]*100;
	TxDat[6] = BYTE1(_temp);
	TxDat[7] = BYTE0(_temp);
	_temp = user_imu_msg.acc[2]*100;
	TxDat[8] = BYTE1(_temp);
	TxDat[9] = BYTE0(_temp);
	_temp = user_imu_msg.gyro[0]*R2D;
	TxDat[10] = BYTE1(_temp);
	TxDat[11] = BYTE0(_temp);
	_temp = user_imu_msg.gyro[1]*R2D;
	//_temp = odometry.Pos[1]*100.0f;
	TxDat[12] = BYTE1(_temp);
	TxDat[13] = BYTE0(_temp);
	_temp = user_imu_msg.gyro[2]*R2D;
	//_temp = odometry.Pos[2]*100.0f;
	TxDat[14] = BYTE1(_temp);
	TxDat[15] = BYTE0(_temp);
	_temp = user_mag_msg.mag[0];
	TxDat[16] = BYTE1(_temp);
	TxDat[17] = BYTE0(_temp);
	_temp = user_mag_msg.mag[1];
	TxDat[18] = BYTE1(_temp);
	TxDat[19] = BYTE0(_temp);
	_temp = user_mag_msg.mag[2];
	TxDat[20] = BYTE1(_temp);
	TxDat[21] = BYTE0(_temp);


	uint8_t sum = 0;
	for(uint8_t i=0; i<22; i++) sum += TxDat[i];
	TxDat[22] = sum;
    return HAL_UART_Transmit_DMA(&huart1, TxDat, 23);

}
